//
// Created by az on 2020/6/20.
//


#include "pos_generator/gazebo_to_pos.h"

void GAZEBO_POS::init(ros::NodeHandle& nh) {
    client_  = nh.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");

    objstate_.request.model_name = "turtlebot3_burger";
    //wait for the client fo gazebo service beginning
    client_.call(objstate_);
    while(!objstate_.response.success){
        ROS_WARN("wait for the gazebo service client !");
        ros::Duration(3).sleep();
        client_.call(objstate_);
        if (objstate_.response.success) {
            ROS_WARN("open the gazebo service client success !");
            break;
        }
    }
}

bool GAZEBO_POS::getPosition(nav_msgs::Odometry& state){
    client_.call(objstate_);

    if (!objstate_.response.success) {
        ROS_ERROR("cant get robot state !");
        ros::Duration(3).sleep();
        return false;
    }

    state.header.frame_id = "wmr";
    state.header.stamp    = ros::Time::now();

    state.pose.pose       = objstate_.response.pose;
    state.twist.twist     = objstate_.response.twist;
    Eigen::Quaterniond quan;
    quan.w() = state.pose.pose.orientation.w;
    quan.x() = state.pose.pose.orientation.x;
    quan.y() = state.pose.pose.orientation.y;
    quan.z() = state.pose.pose.orientation.z;

    Eigen::Matrix3d DCM;
    Eigen::Vector3d Eular;
    OriTrans::Quater2DCM(quan, DCM);
    OriTrans::DCM2Euler(DCM, Eular);
    //当前机器人的信息
    state_lock_.lock();
    robot_yaw_ = Eular(2);
    state_     = state;
    state_lock_.unlock();

    has_pos_ = true;
    return true;
}

bool GAZEBO_POS::getCloudPoint(const sensor_msgs::LaserScan &msg, sensor_msgs::PointCloud2 &obj,
                               sensor_msgs::PointCloud2 &ground) {
    if(!has_pos_) return false;
    obj.header.stamp    = ros::Time::now();
    ground.header.stamp = ros::Time::now();

    state_lock_.lock();
    double& x = state_.pose.pose.position.x;
    double& y = state_.pose.pose.position.y;

    Eigen::Vector3d rob_pt(x, y, robot_yaw_);
    state_lock_.unlock();

    pcl::PointCloud<pcl::PointXYZ> pcl_obj, pcl_ground;
    pcl::PointXYZ obj_pt, ground_pt;

    double angle = msg.angle_min;
    for(int i = 0; i < (int)msg.ranges.size(); i++){
        angle += msg.angle_increment;
        if(!std::isinf(msg.ranges[i])){
            double d = msg.ranges[i];
            if(d > msg.range_min && d <= msg.range_max){
                obj_pt.x = (float)(d * cos(angle));
                obj_pt.y = (float)(d * sin(angle));
                obj_pt.z = 0.1;
                pcl_obj.points.push_back(obj_pt);
            }
        } else {
            double d = msg.range_max;

            ground_pt.x = (float)(d * cos(angle));
            ground_pt.y = (float)(d * sin(angle));
            ground_pt.z = 0.1;
            pcl_ground.points.push_back(ground_pt);
        }
    }

    pcl_obj.width    = pcl_obj.points.size();
    pcl_obj.height   = 1;
    pcl_obj.is_dense = true;
    pcl_ground.width    = pcl_ground.points.size();
    pcl_ground.height   = 1;
    pcl_ground.is_dense = true;

    pcl::toROSMsg(pcl_obj,    obj);
    pcl::toROSMsg(pcl_ground, ground);
    obj.header.frame_id    = "odom";
    ground.header.frame_id = "odom";
    obj.header.stamp    = msg.header.stamp;
    ground.header.stamp = msg.header.stamp;

    return true;
}
